/**
 ******************************************************************************
 * @file   task.cpp
 * @brief  freertos task running file.
 ******************************************************************************
 * @note
 *  - Before running your devices, just do what you want ~ !
 *  - More devices or using other classification is decided by yourself ~ !
 ===============================================================================
                                   Task List
 ===============================================================================
 * <table>
 * <tr><th>Task Name     <th>Priority          <th>Frequency/Hz    <th>Stack/Byte
 * <tr><td>              <td>                  <td>                <td>
 * </table>
 *
*/

/* Includes ------------------------------------------------------------------*/
#include "internal.h"
/* Private define ------------------------------------------------------------*/
TaskHandle_t DjiMotor_Handle;
TaskHandle_t IMU_Handle;
TaskHandle_t DR16_Handle;
/* Private function declarations ---------------------------------------------*/
void tskDjiMotor(void *arg);
void tskIMU(void *arg);
void tskDR16(void *arg);
/* Function prototypes -------------------------------------------------------*/
/**
 * @brief  Initialization of device management service
 * @param  None.
 * @return None.
 */
void Service_Devices_Init(void)
{
  xTaskCreate(tskDjiMotor, "App.Motor", Small_Stack_Size, NULL, PriorityAboveNormal, &DjiMotor_Handle);
#if USE_SRML_MPU6050
  xTaskCreate(tskIMU, "App.IMU", Small_Stack_Size, NULL, PriorityNormal, &IMU_Handle);
#endif
  xTaskCreate(tskDR16, "App.DR16", Small_Stack_Size, NULL, PriorityAboveNormal, &DR16_Handle);
}

/**
 * @brief <freertos> 大疆电机控制任务
 */
void tskDjiMotor(void *arg)
{
  /*	pre load for task	*/
  static Motor_CAN_COB Tx_Buff;
  static int16_t AngleStep_cnt = 0;
  float test1 = 0;
  float test2 = 0;

  PitchPID_Angle.Target = 0;
  YawPID_Angle.Target = 0;

  PitchPID_Speed.SetPIDParam(250, 0, 0, 10000, 30000);
  PitchPID_Angle.SetPIDParam(18, 0.05, 0, 100, 30000);

  YawPID_Speed.SetPIDParam(200, 0, 0, 10000, 30000);
  YawPID_Angle.SetPIDParam(20, 0.1, 0, 100, 30000);

  LALPID_Speed.SetPIDParam(20, 0, 0, 0.8, 16000);
  LALPID_Angle.SetPIDParam(8, 0, 0.05, 100, 10000);

  AccePIDL_Speed.SetPIDParam(7, 0, 0, 20, 10000);
  AccePIDR_Speed.SetPIDParam(7, 0, 0, 20, 10000);

  PitchMotor.setEncoderOffset(6600);
  for (;;)
  {
    /* wait for next circle */
    vTaskDelay(1);
    /*	电机控制	*/
    if (DR16.GetStatus() == DR16_ESTABLISHED)
    {
      AngleStep_cnt++;
      switch (DR16.GetS2())
      {
      case DR16_SW_UP:
        AngleStep_cnt = 0;
        break;
      case DR16_SW_MID:
        if (AngleStep_cnt == 1000)
        {
          LALPID_Angle.Target -= 36 * 36; //减速比*一次一个角度
        }
        break;
      case DR16_SW_DOWN:
        if (AngleStep_cnt % 100 == 0)
        {
          LALPID_Angle.Target -= 36 * 36;
        }
        break;
      default:
        break;
      }
      if (AngleStep_cnt == 1000)
        AngleStep_cnt = 0;

      PitchPID_Speed.Target = PitchPID_Angle.Adjust();
      PitchPID_Speed.Current = -mpu_receive.gyro[1];
      PitchMotor.Out = PitchPID_Speed.Adjust();

      YawPID_Speed.Target = YawPID_Angle.Adjust();
      YawPID_Speed.Current = mpu_receive.gyro[2];
      YawMotor.Out = YawPID_Speed.Adjust();

      LALPID_Speed.Target = LALPID_Angle.Adjust();
      LALMotor.Out = LALPID_Speed.Adjust();
    }
    else
    {
      AccePIDL_Speed.Target = 0;
      AccePIDR_Speed.Target = 0;
      LALMotor.Out = 0;
    }

    AcceMotorL.Out = AccePIDL_Speed.Adjust();

    AcceMotorR.Out = AccePIDR_Speed.Adjust();

    // if (DR16.GetStatus() != DR16_ESTABLISHED)
    // {
    //   AccePIDL_Speed.Target = 0;
    //   AccePIDR_Speed.Target = 0;
    // }
    // else
    // {
    // }

    /*	将电机输出数据打包成can消息队列	*/

    Tx_Buff = MotorMsgPack(Tx_Buff,
                           LALMotor, AcceMotorL, AcceMotorR); //	yaw轴电机

    //	发送can队列，根据电机的发射帧id选择需要发送的数据包
    xQueueSend(CAN1_TxPort, &Tx_Buff.Id1ff, 0);
    xQueueSend(CAN2_TxPort, &Tx_Buff.Id2ff, 0);
    xQueueSend(CAN1_TxPort, &Tx_Buff.Id200, 0);
  }
}

#if USE_SRML_MPU6050
/**
 * @brief MPU6050读取数据
 */
void tskIMU(void *arg)
{
  /* Pre-Load for task */
  TickType_t xLastWakeTime_t;
  xLastWakeTime_t = xTaskGetTickCount();
  for (;;)
  {
    /* wait for next circle */
    vTaskDelayUntil(&xLastWakeTime_t, 5);
    /*	读取MPU6050数据	*/
    dmp_read_data(&mpu_receive);
    // printf("gyroX,Y,Z:%.2f,%.2f,%.2f\r\n",
    // mpu_receive.gyro[0],
    // mpu_receive.gyro[1],
    // mpu_receive.gyro[2]);
    // printf("accelX,Y,Z:%.2f,%.2f,%.2f\r\n",
    // mpu_receive.accel[0],
    // mpu_receive.accel[1],
    // mpu_receive.accel[2]);
    // printf("angle Pitch,Yaw,Roll:%.2f,%.2f,%.2f\r\n",
    // mpu_receive.pitch,
    // mpu_receive.yaw,
    // mpu_receive.roll);
  }
}
#endif

/**
 *	@brief	Dr16 data receive task
 */
void tskDR16(void *arg)
{
  /* Cache for Task */
  static USART_COB Rx_Package;
  /* Pre-Load for task */
  DR16.Check_Link(xTaskGetTickCount());

  /* Infinite loop */
  for (;;)
  {
    /* Enter critical */
    // xSemaphoreTake(DR16_mutex, portMAX_DELAY);
    /*	等待数据	*/
    if (xQueueReceive(DR16_QueueHandle, &Rx_Package, 100) == pdPASS)
    {
      DR16.DataCapture((DR16_DataPack_Typedef *)Rx_Package.address);
    }
    portENTER_CRITICAL();
    /*	检测遥控器连接 */
    DR16.Check_Link(xTaskGetTickCount());
    portEXIT_CRITICAL();
    /*	判断是否连接 	 */
    if (DR16.GetStatus() != DR16_ESTABLISHED)
    {
      /* Leave critical */
      // xSemaphoreGive(DR16_mutex);
      continue;
    }
    /*	更新遥控器控制	*/
    if (DR16.Get_LY_Norm() != 0.f)
      PitchPID_Angle.Target = DR16.Get_LY_Norm() * 30.0f;
    else
      PitchPID_Angle.Target = 0;

    YawPID_Angle.Target += DR16.Get_LX_Norm() * 3;

    switch (DR16.GetS1())
    {
    case DR16_SW_MID:
      AccePIDL_Speed.Target = 0;
      AccePIDR_Speed.Target = 0;
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_15, GPIO_PIN_RESET);
      break;
    case DR16_SW_UP:
      AccePIDL_Speed.Target = 3900;
      AccePIDR_Speed.Target = -3900;
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_15, GPIO_PIN_SET);
      break;
    case DR16_SW_DOWN:
      AccePIDL_Speed.Target = 4320;
      AccePIDR_Speed.Target = -4320;
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_15, GPIO_PIN_SET);
      break;
    }

    // Sent_Contorl(&huart4,
    //              PitchMotor.getAngle(),
    //              PitchPID_Angle.Target,
    //              YawMotor.getAngle(),
    //              YawPID_Angle.Target,
    //              0,
    //              0);
    /* Leave critical */
    // xSemaphoreGive(DR16_mutex);
  }
}
